#include "filter.h"
  /**************************************************************************
作者：平衡小车之家
我的淘宝小店：http://shop114407458.taobao.com/
**************************************************************************/
//float K1 =0.02f;
float angle_x,angle_y;
//float Q_angle=0.001f;// 过程噪声的协方差
//float Q_gyro=0.003f;//0.003 过程噪声的协方差 过程噪声的协方差为一个一行两列矩阵
//float R_angle=0.5f;// 测量噪声的协方差 既测量偏差
//float dt=0.03f;//
//char  C_0 = 1;
//float Q_bias, Angle_err;
//float PCt_0, PCt_1, E;
//float K_0, K_1, t_0, t_1;
//float Pdot[4] ={0,0,0,0};
//float PP[2][2] = { { 1, 0 },{ 0, 1 } };

/**************************************************************************
函数功能：简易卡尔曼滤波
入口参数：加速度、角速度
返回  值：无
**************************************************************************/



/**************************************************************************
函数功能：一阶互补滤波
入口参数：加速度、角速度
返回  值：无
**************************************************************************/
void Yijielvbo_x(float angle_m, float gyro_m)
{
static      float K1 =0.02f;
   angle_x = K1 * angle_m+ (1-K1) * (angle_x + gyro_m * 0.01f);
}



/**************************************************************************
函数功能：一阶互补滤波
入口参数：加速度、角速度
返回  值：无
**************************************************************************/
void Yijielvbo_y(float angle_m, float gyro_m)
{
static      float K1 =0.02f;
   angle_y = K1 * angle_m+ (1-K1) * (angle_y + gyro_m * 0.01f);
}


